package com.rockwell.robot2015.model.states.running.sequence;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.sensors.IRobotSensors;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;

public class EnteringMazeSequence extends RobotRunningSequence {
	/**
	 * The distance where we can safely pick the treasure
	 */
	private final static int SMALL_TURN_INDEX = 5;

	/**
	 * Alignment threshold beyond which we should adjust the orientation of the
	 * robot.
	 */
	private final static double ALIGNMENT_THRESHOLD = 5;

	enum WallAlignment {
		Lost, TooLeft, TooRight, TurnRight, TurnLeft, Aligned
	}

	enum State implements IState {
		Start {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		TurnLeft {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().sendCommand(RobotAction.TurnLeft);
				machine.changeState(Jog);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		Jog {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void move(IStateMachine machine) {
				IDetector detector = machine.robot()
						.getMazeEntranceGuideDetector();
				machine.robot().sendCommand(RobotAction.Jog);

				switch (getAlignment(machine)) {
				case Lost:
					machine.changeState(WallLost);
					break;
				case TooLeft:
					machine.changeState(NudgeRight);
					break;
				case TooRight:
					machine.changeState(NudgeLeft);
					break;
				case Aligned:
					if (detector.isPresent()) {
						machine.changeState(AlignLine);
					} else {
						machine.changeState(Jog);
					}
					break;
				case TurnRight:
					machine.robot().sendCommand(RobotAction.TurnRight);
					machine.changeState(Jog);
					break;
				case TurnLeft:
					machine.changeState(TurnLeft);
					break;

				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		NudgeRight {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().sendCommand(RobotAction.TurnDegree,
						SMALL_TURN_INDEX);
				machine.changeState(Jog);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		NudgeLeft {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().sendCommand(RobotAction.TurnDegree,
						-SMALL_TURN_INDEX);
				machine.changeState(Jog);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		AlignLine {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void move(IStateMachine machine) {
				IDetector detector = machine.robot()
						.getMazeEntranceGuideDetector();
				machine.robot().sendCommand(RobotAction.MoveWithSensor, 30);
				machine.robot().sendCommand(RobotAction.TurnRight);
				// may change drift to y, needs to check with screen
				while (Math.abs(detector.getDrift().y) > ALIGNMENT_THRESHOLD) {
					if (detector.getDrift().y > ALIGNMENT_THRESHOLD) {
						machine.robot().sendCommand(RobotAction.TurnDegree,
								-SMALL_TURN_INDEX);
					} else if (detector.getDrift().y < -ALIGNMENT_THRESHOLD) {
						machine.robot().sendCommand(RobotAction.TurnDegree,
								SMALL_TURN_INDEX);
					}
				}
				machine.robot().sendCommand(RobotAction.MoveWithSensor, 30);
				machine.changeState(EnterMazeSuccess);

			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		EnterMazeSuccess {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		WallLost {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		};

		static EnteringMazeSequence m_sequence;

		public static void setSequence(EnteringMazeSequence sequence) {
			m_sequence = sequence;
		}

		public static WallAlignment getAlignment(IStateMachine machine) {
			IRobotSensors sensor = machine.robot().getSensor();
			if (sensor.getRightDistance() > 20.0) {
				return WallAlignment.TooLeft;
			} else if (sensor.getLeftDistance() < 5.0) {
				return WallAlignment.TooRight;
			} else if (sensor.detectRightWall() == false) {
				return WallAlignment.TurnRight;
			} else if (sensor.detectRightWall() && sensor.detectFrontWall()) {
				return WallAlignment.TurnLeft;
			} else
				return WallAlignment.Aligned;
		}
	}

	public EnteringMazeSequence(RobotRunningStateMachine running) {
		super(running, State.Start);

		State.setSequence(this);
		m_currState = State.Start;
	}

	@Override
	public void start() {
		m_currState = State.Start;
		m_nextState = null;
		
		Robot robot = m_running.robot();
		robot.enableTreasureDetector(false);
		//robot.enableObstacleDetector(false);
		robot.enableMaze1Detector(false);
		robot.enableMaze2Detector(false);
		robot.enableMaze3Detector(false);
		robot.enableMazeEntranceGuideDetector(true);
		robot.enableDropAreaDetector(false);
		robot.enableOuterWallDetector(false);
	}

	@Override
	public boolean succeeded() {
		return m_currState.equals(State.EnterMazeSuccess);
	}

	@Override
	public boolean failed() {
		// TODO Auto-generated method stub
		return false;
	}
}
